#include "Angle.h"
#include <math.h>

namespace common {
Angle::Angle(int id,Point* k,Point* l,Point* m,Point* n,Parameter* param):common::Constraint(id,1,ANGLE){
	setK(k);
	setL(l);
	setM(m);
	setN(n);
	setParameter(param);

}
ublasVector Angle::getValue() throw(MyException){

	//(L-K)'*(N-M)-cos(param)*|L-K|*|N-M| = 0

	ublasVector v(1);
	Vector vLK = (*getL()-*getK());
	Vector vNM = (*getN()-*getM());

	v(0) = vLK*vNM  -cos(getParameter()->getValue()*M_PI/180)*vLK.length()*vNM.length();
	return v;

}
ublasMatrix Angle::getJacobianOnPoint(Point* p) throw(MyException){

	ublasMatrix mout(1,2);

	Vector LK_ = (*getL()-*getK());
	Vector NM_ = (*getN()-*getM());
	Vector vLK_ = LK_.unit();
	Vector vNM_ = NM_.unit();

	ublasVector LK(2); ublasVector vLK(2);
	ublasVector NM(2); ublasVector vNM(2);
	LK << LK_; NM << NM_;
	vLK << vLK_; vNM << vNM_;
	skreal pv = getParameter()->getValue()*M_PI/180;

	if(k==p){
		row(mout,0)= -NM + cos(pv)*NM_.length()*vLK;
	}
	else if(l==p){
		row(mout,0)=  NM - cos(pv)*NM_.length()*vLK;
	}
	else if(m==p){
		row(mout,0)= -LK + cos(pv)*LK_.length()*vNM;
	}
	else if(n==p){
		row(mout,0)=  LK - cos(pv)*LK_.length()*vNM;
	}

	return mout;
}
ublasVector Angle::getNorm() throw(MyException){
	return ublasVectorZero(1);
}
ublasMatrix Angle::getHessian(Point* s, Point* r) throw(MyException){

	ublasMatrix out(ublasMatrixZero(2,2));
	ublasMatrixIdentity I(2,2);

	Vector LK_ = (*getL()-*getK());
	Vector NM_ = (*getN()-*getM());
	Vector vLK_ = LK_.unit();
	Vector vNM_ = NM_.unit();

	ublasVector LK(2); ublasVector vLK(2);
	ublasVector NM(2); ublasVector vNM(2);
	LK << LK_; NM << NM_;
	vLK << vLK_; vNM << vNM_;
	skreal pv = getParameter()->getValue()*M_PI/180;
	skreal g = cos(pv)*inner_prod(NM,LK);

	if(k==s && m==r){
		out = I - I*g;
	}else if(k==s && n==r){
		out =-I + I*g;
	}else if(l==s && m==r){
		out =-I + I*g;
	}else if(l==s && n==r){
		out = I - I*g;
	}else if(m==s && k==r){
		out =-I + I*g;
	}else if(m==s && l==r){
		out =-I + I*g;
	}else if(n==s && k==r){
		out =-I + I*g;
	}else if(n==s && l==r){
		out =-I + I*g;
	}else {
		return ublasMatrixZero(2,2);
	}


	return out;
}
bool Angle::isHessian() const{
	return true;
}
Angle::~Angle(){

}
}
